﻿using System;
using System.Collections.Generic;
using System.Text;
using G3.Controller;
using G3.State;

namespace G3.AI
{
    #region HumanDriver
    public class HumanDriver : IDriver
    {
        private double wheelPosition = 0;
        private double throttlePosition = 0;
        private bool   acceleration = false;
        private bool   braking = false;
        private bool   left = false;
        private bool   right = false;
        private double HumanDriverAcceleration;
        private double HumanDriverBraking;
        private double HumanDriverEngineBraking;
        private double HumanDriverWheelSteering;
        private double HumanDriverWheelRetraction;

        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
        G3.Controller.Controller myController;
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//

        public HumanDriver()
        {
            HumanDriverAcceleration = int.Parse((String)Parameters.GetParameter("HumanDriverAcceleration"));
            HumanDriverBraking = int.Parse((String)Parameters.GetParameter("HumanDriverBraking"));
            HumanDriverEngineBraking = int.Parse((String)Parameters.GetParameter("HumanDriverEngineBraking"));
            HumanDriverWheelSteering = int.Parse((String)Parameters.GetParameter("HumanDriverWheelSteering"));
            HumanDriverWheelRetraction = int.Parse((String)Parameters.GetParameter("HumanDriverWheelRetraction"));

            //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
            myController = Controller.Controller.GetOnlyInstance();
            //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
        }

        public Pair Think()
        {
            //Applying interactions
            if (acceleration)                                           //accelerating
            {
                throttlePosition += HumanDriverAcceleration;
                if (throttlePosition > 100) throttlePosition = 100;
            }
            else                                                                //not...
            {
                if (throttlePosition > 0) throttlePosition -= HumanDriverEngineBraking;
                if (!braking && throttlePosition < 0) throttlePosition = 0;
            }

            if (braking)                                                //braking
            {
                throttlePosition -= HumanDriverBraking;
                if (throttlePosition < -100) throttlePosition = -100;
            }

            if (left)              //steering left
            {
                wheelPosition -= HumanDriverWheelSteering;
                if (wheelPosition < -100) wheelPosition = -100;
            }
            else if (right)         //steering right
            {
                wheelPosition += HumanDriverWheelSteering;
                if (wheelPosition > 100) wheelPosition = 100;
            }
            else                                        //retracting wheel to the centre
            {
                if (Math.Abs(wheelPosition) < HumanDriverWheelRetraction || wheelPosition == 0) wheelPosition = 0;
                else wheelPosition -= Math.Sign(wheelPosition) * HumanDriverWheelRetraction;
            }

            //Returning with the current positions
            myController.SetWheelThrootlePositions(throttlePosition, wheelPosition);            
            return new Pair(throttlePosition, wheelPosition);
        }

        public void Teach(int treshold)
        {
        }

        public void LoadProfile(String fileName)
        {
        }

        public void SaveProfile(String fileName)
        {
        }

        public DriverType Type()
        {
            return DriverType.Human;            
        }

        public void HandleInteraction(System.Windows.Forms.Keys KeyCode, bool pushed)         //handling interaction idetified by a char which is sent by form, second parameter is true, if the key is pushed, and false if isn't
        {
            switch (KeyCode)
            {
                /*
                case System.Windows.Forms.Keys.Up:      acceleration    = pushed; break;
                case System.Windows.Forms.Keys.Space:   braking         = pushed; break;
                case System.Windows.Forms.Keys.Left:    left            = pushed; break;
                case System.Windows.Forms.Keys.Right:   right           = pushed; break;
                */

                case System.Windows.Forms.Keys.W: acceleration = pushed; break;
                case System.Windows.Forms.Keys.S:
                    {
                        braking = pushed;
                        if (pushed && throttlePosition>0 ) throttlePosition = 0;
                        if (!pushed && throttlePosition < 0) throttlePosition = 0;
                        break;
                    }
                case System.Windows.Forms.Keys.A: left = pushed; break;
                case System.Windows.Forms.Keys.D: right = pushed; break;
            }


            //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
            //Pair positions = Think();
            //myController.SetWheelThrootlePositions(positions.x, positions.y);            
            //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
        }
    }
    #endregion
}
